#include <memory>
#include <boost/asio.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/system/error_code.hpp>
#include <boost/system/system_error.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/tf.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int8MultiArray.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/BatteryState.h>


#define SERIAL_HEAD_A 0xFF
#define SERIAL_HEAD_B 0x55

typedef boost::shared_ptr<boost::asio::serial_port> serialp_ptr;


#define SIZE_OF_RX_PROTOCOL 81
#pragma pack(push,1)
struct RX_PROTOCOL{
    uint8_t Header1;
	uint8_t Header2;
	int16_t  X_Angle_Speed;
	int16_t  Y_Angle_Speed;
	int16_t  Z_Angle_Speed;
	int16_t  X_Acceleration;
	int16_t  Y_Acceleration;
	int16_t  Z_Acceleration;
	int32_t  Odom[3];
    int16_t  Speed[2];
	int32_t  L_Motor_Speed;
	int32_t  R_Motor_Speed;
	int32_t  L_Motor_Position;
	int32_t  R_Motor_Position;
	uint8_t  Drop_Sensor[6];
	int16_t  L_Motor_Temperature;
	int16_t  R_Motor_Temperature;
	int16_t  L_Motor_Current;
	int16_t  R_Motor_Current;
	int16_t  R_Motor_Error_Code;		
	int16_t  L_Motor_Error_Code;	
	int16_t  Charge_Current;
	int16_t  Battery_Current;
	int16_t  Battery_Voltage;
	uint16_t Robot_Status;
    uint16_t MOTOR_L_P;
	uint16_t MOTOR_L_I;
	uint16_t MOTOR_R_P;
	uint16_t MOTOR_R_I;
	uint8_t  Check_Sum;
};
#pragma pack()

enum packetFinderState
{
    waitingForHead1,
    waitingForHead2,
    waitingForPayload,
    waitingForCheckSum,
    handlePayload
};


class RaymoDriver
{
    public:
     RaymoDriver();
     ~RaymoDriver();
     void loop();

    private:
     bool initRobot();
     void recv_msg();
     void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
     void estop_callback(const std_msgs::Bool::ConstPtr& msg);
     void send_speed_callback(const ros::TimerEvent&);
     void handle_base_data(const uint8_t* buffer_data);
     void check_sum(uint8_t* data, size_t len, uint8_t& dest);
     void distribute_data(uint8_t* buffer_data);
     
     bool SetMotorSpeed(int16_t linear_speed, int16_t angular_speed);
     bool SetEStop(bool estop);

     void monitor_data(void);

     packetFinderState state_;
     boost::mutex cmd_vel_mutex_;
     boost::system::error_code ec_;
     boost::asio::io_service io_service_;
     boost::mutex mutex_;
     serialp_ptr sp_;

     bool recv_flag_;
     bool publish_odom_transform_;
     bool start_flag_;
     bool first_init_;
     uint8_t msg_seq_;
     geometry_msgs::Twist current_twist_;
     nav_msgs::Odometry odom_pub_data_;
     geometry_msgs::TransformStamped transformStamped_;
     tf2_ros::TransformBroadcaster br_;

     ros::Time now_;
     ros::Time last_time_;
     ros::Time last_twist_time_;

     std::string port_name_;
        
     int baud_rate_;

     struct RX_PROTOCOL rx_data_;

     std::string odom_frame_;
     std::string imu_frame_;
     std::string base_frame_;
     std::string code_version_;
     int control_rate_;

     ros::Publisher odom_pub_;
     ros::Publisher battery_pub_;
     ros::Publisher imu_pub_;
     ros::Publisher battery_state_pub_;
     ros::Publisher charge_current_pub_;
     ros::Publisher robot_status_pub_;
     ros::Publisher motor_l_speed_pub_;
     ros::Publisher motor_r_speed_pub_;
     ros::Publisher drop_sensor_pub_;

     std_msgs::Float32 motor_l_speed_pub_data_;
     std_msgs::Float32 motor_r_speed_pub_data_;

     ros::Subscriber cmd_sub_;
     ros::Subscriber estop_sub_;

     sensor_msgs::Imu imu_pub_data_;
     
     std_msgs::Float32   charge_current_data_;
     
     sensor_msgs::BatteryState battery_state_pub_data_;

     int32_t l_motor_pos_offset_;
     int32_t r_motor_pos_offset_;

     int32_t l_motor_pos_;
     int32_t r_motor_pos_;

     int32_t l_motor_speed_;
     int32_t r_motor_speed_;

     int default_acc_;
     int default_dacc_;

     bool motor_pos_initial_;

};
